Several control systems employed by vehicles, either autonomous vehicles or vehicles executing in autonomous-driving mode, predict future, safe motions, or paths, of the vehicle, both in order to avoid obstacles, such as other vehicles or pedestrians, but also to optimize some criteria associated to the operation of the vehicle. The vehicle is controlled along the path that is a part of the object trajectory or, more precisely, the projection of the trajectory onto the two-dimensional plane of movement. To say it more figuratively, the path is the line connecting all geometrical coordinates of the moving vehicle.
A fundamental objective of path planning is to construct a trajectory connecting two states, e.g., an initial state and a target state, of a moving object, such as autonomous or semi-autonomous vehicles executing in autonomous-driving mode, and to control the motion of the vehicles along the trajectory. A “state” as used herein is a vector including the geometrical coordinates of the vehicle in the plane and an orientation of the vehicle at these geometrical coordinates. The target state can either be a fixed location, a moving location, a velocity vector, a region, or a combination thereof. The surroundings, such as road edges, pedestrians, and other vehicles, are sensed by the sensors of the vehicle and/or are at least partially known by a priori given information.
A path connecting an initial state and a target state is normally made up of several elementary paths (or EPT for short). In some situations, the path planning solely works with elementary paths of the same or different kinds. To that end, the path of a moving body is normally made up of a plurality of elementary paths that share the same junction points (abbreviating to JP). Thus, the junction points are special points of the path because each of them belongs to two adjoining elementary paths (EPT).
One special characteristic demanded of the paths is that states do not change erratically along the entire course or at the junction points. This is to say that the transition from one elementary path to an adjacent elementary path must be ensured to be continuous at the junction points of the elementary paths as well as at its first derivative (tangent) and its second derivative (curvature). The path is continuous at junction points when the transition from one elementary path to the adjacent elementary path can be performed by changing a steering of the vehicle in a linear manner and/or by switching a sign of a velocity of the vehicle.
For example, pioneering work Dubins and Reeds and Shepp investigated patterns of a shortest path between two states of the vehicle. The shortest path, termed Reeds-Shepp's (RS) path, is a sequential composition of line segments and/or tangential circular arcs of a minimum turning radius. The RS path can be computed in very efficient way, but the curvature along the RS path involving multiple segments is discontinuous. Such discontinuity of the curvature is undesirable in practice, since a vehicle has to stop and perform stationary steering, leading to unnecessary time delay and extra wearing of tires.
Some methods overcome discontinuity of the curvature in RS paths by introducing other segments, in addition to the line and circular segments. For example, the path planning can combine various types of elementary paths such as splines, polynomials, trigonometric functions or clothoids. The aforementioned elementary paths are mainly distinguished by their mathematical description. A feature that all elementary paths have in common is that they can be combined with straight and/or circular lines to form a continuous path.
Some conventional methods prefer to base its path planning on clothoids because they are closely related to the rules of movement of steered motor vehicles. A clothoid is a special type of plane curve that is marked by its curvature, c, changing in a linear manner. Clothoids have the added benefit of the length of the path being minimized when an initial state is connected with a terminal state. However, a continuous-curvature path typically includes a sequence of multiple elementary paths, and it can be difficult to find the parameters of the optimal sequence forming the continuous path.
For example, method described in U.S. 2016/03133735 first determines an RS path and tries to find a continuous-curvature path which stays as close as possible to the RS path. This method computes the continuous-curvature path by solving optimization problems with nonlinear equations. However, the solution to this method requires iterative process, which can be computationally prohibitive for a number of practical applications. Also, the optimization problem of that method is non-convex. The solution to the non-convex problem is not guaranteed, and thus the continuous-curvature path may not be constructed. Hence, this method may not be suitable for a real-time control of the vehicles. Similarly, the method described in U.S. Pat. No. 8,428,820 improves the computation efficiency, but still entails computationally expensive iterative process to construct the continuous path. Hence, this method also may not be suitable for a real time control of the vehicles.
Accordingly, there is a need to construct a continuous-curvature path for controlling a motion of a vehicle from an initial to a target state in a manner suitable for real-time control of the vehicles.